/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 *
 * $Id: Behaviour_Dummy.cpp,v 1.8 2009-10-22 13:21:09 morsko Exp $
 *
 * \file
 * \brief Dummy implementation for Behaviour Server interface.
 * \author Matthieu Myrsky <matthieu.myrsky@tkk.fi>
 *
 * This implementation can act as an example / codebase for any
 * Behaviour server based module implementation. This code follows a
 * pseudogood coding style, and also provides a basic parameter
 * parser that can be directly used with slight modifications.
 * 
 */
#include <stdio.h>
#include <stdlib.h>
#include "owndebug.h"
#include "BehaviourServer.hpp"
#include "binbag.h"

//*****************************************************************************
#define GIMI_CLIENT_API (20000)
#include "gimi.h"
//*****************************************************************************
static int verbose = 1;
//*****************************************************************************
using namespace MaCI;
//*****************************************************************************

void showUsage()
{
  fprintf(stderr,"\n");
  fprintf(stderr,"Usage:\tBehaviour_Dummy [OPTIONS]\n");
  fprintf(stderr,"  [-u addr]\t\t\tUse 'addr' as GIMnet AccessPoints address\n");
  fprintf(stderr,"  [-p port]\t\t\tUse 'port' as GIMnet AccessPoints port\n");
  fprintf(stderr,"  [-n name]\t\t\tUse 'name' as this nodes GIMnet name\n");
  fprintf(stderr,"  [-G name]\t\t\tUse 'name' as MaCIGroup for this service\n");
  fprintf(stderr,"  [-I name]\t\t\tUse 'name' as Instancename for this service\n");

  fprintf(stderr,"  [-v]\t\t\t\tIncrease verbosity by 1\n");
  fprintf(stderr,"  [-q]\t\t\t\tQuiet mode\n");
  fprintf(stderr, "\n\n");
}
//*****************************************************************************

int main(int argc, char *argv[]) 
{
 // namespace to use
  using namespace MaCI::Behaviour;
  
  // GIMnet parameters
  std::string gimnetAP = "asrobo.hut.fi";
  int gimnetAPPort = 50002;
  std::string gimnetName = "";
  std::string maciGroupName;
  std::string interfaceInstanceName = "Dummy";
  // Objects
  bool run = true;
  gimi::GIMI g;
  MaCICtrl::CMaCICtrlServer mcs(&g);
  Behaviour::CBehaviourServer bs(&mcs, 0, "Very Very Dummy"); 

  // Init Debug lib
  debugInit();
  //debugSetGlobalDebugLvl(4);
  debugSetLogFilename("Behaviour_Dummy.log");

  // Greet
  fprintf(stderr,
	  "\nBehaviour_Dummy - MaCI Behaviour Interface implementation for Dummy - v0.0\n");

  // Parse parameters
  bool parse = true;
  while(parse) {
    int c;
    c = getopt(argc, argv, "I:G:n:u:p:hqv");
    switch(c) {

      /////////////////////////////////
      ////// Application specific /////
      /////////////////////////////////

      /////////////////////////////////
      ///// GIMnet parameters /////////
      /////////////////////////////////
    case 'n':
      gimnetName = optarg;
      dPrint(3,"Using GIMnet Name '%s'", optarg);
      break;

    case 'u':
      gimnetAP = optarg;
      dPrint(3,"Using GIMnetAP address '%s'", optarg);
      break;

    case 'p':
      gimnetAPPort = atoi(optarg);
      dPrint(3,"Using GIMnetAP port '%d'", gimnetAPPort);
      break;
  
    case 'G':
      maciGroupName = optarg;
      dPrint(3,"Using MaCIGroupName '%s'", optarg);
      break;

    case 'I':
      interfaceInstanceName = optarg;
      dPrint(3,"Using InterfaceInstance Name '%s'", optarg);
      break;
      //////////////////////////////////
      //// GENERIC:                 ////
      //////////////////////////////////
    case 'v':
      // Increase only if not previously set to 0 (by 'quiet')
      if (verbose > 0)
	debugSetGlobalDebugLvl(++verbose);
      break;
      
    case 'q':
      verbose = 0;
      debugSetGlobalDebugLvl(0);
      break;
      
    case 'h':
    case '?':
      showUsage();
      exit(1);
      break;
      
    case -1:
      parse = false;
      break;
    }
  }
  mcs.SetGroupName(maciGroupName);
  bs.SetInterfaceInstanceName(interfaceInstanceName);
  int r;
  if ( (r = g.connectToHubEx(gimnetAP, gimnetAPPort, gimnetName) ) != GIMI_OK) {
    dPrint(1,"Failed to connect to GIMnet AccessPoint '%s:%d' with name '%s': '%s'",
	   gimnetAP.c_str(), 
	   gimnetAPPort, 
	   gimnetName.size()?gimnetName.c_str():"<anonymous>",
	   gimi::getGimiErrorString(r).c_str());
    
  } else {
    dPrint(2,"Succesfully connected to GIMnet at AccessPoint '%s:%d' with name '%s': '%s'",
	   gimnetAP.c_str(), 
	   gimnetAPPort, 
	   gimnetName.size()?gimnetName.c_str():"<anonymous>",
	   gimi::getGimiErrorString(r).c_str());
    
    MaCI::EMaCIError e;
    bs.SetBehaviourType(KBehaviourTypeAiding);
    if ( (e = bs.Open()) != MaCI::KMaCIOK) {
      dPrint(1,"Failed to Open BehaviourServer instance! (%s)",
	     GetErrorStr(e).c_str());
      
    } else {


      ////// Execute test section ///////
        int interval = 20;
      
      while(run) {
        CBehaviourData bData;
        gim::binbag::CBinBag bag;
	bData.EncodeTo(&bag);
        bData.Reset();
        float speed = 1.1;
        float angle = 3.12;
        bData.SetCommand(TCommand( KCommandBehaviourControlEvent));
        bData.SetBehaviourControl(TBehaviourControl(speed,angle));
        bs.SendBehaviourControlEvent(bData);
        dPrint(6,"New behaviour control event sent!");
        bData.Print(6);

        //Do Wait
        CBehaviourData bd;
        e = bs.WaitCommand(bd, interval);
        if (e == MaCI::KMaCIOK) {
          bool behaviourRunning = true;
          dPrint(6,"Got Command!");
          const TCommand *cmd = bd.GetCommand();
          if (cmd) {
            switch(cmd->cmd) {
            case KCommandStart:
              dPrint(6,"Got Command, SET START");
              behaviourRunning = true;
              bs.SendBehaviourStateEvent(true);
              break;
            case KCommandStop:
              dPrint(6,"Got Command, SET STOP");
              behaviourRunning = false;
              bs.SendBehaviourStateEvent(false);
              break;
            case KCommandGetBehaviourStateEvent:{
              bs.SendBehaviourStateEvent(behaviourRunning);
              break;

            }
            }
          } else {
            dPrint(1,"No Command in Command!?");
          }
        }
      }

      ////// Test section end //////
    }


  }

  return 0;
}
